#include "XenoCollide2.h"
#include "MPRKernel2.h"

//for debug
#include <windows.h>
#include <GL/gl.h>

class PortalDrawerGL : public MPRObserver
{
public:
	virtual void OnPhase1Started(const MinkowskiDifference2& difference, const Vector2& v0)
	{
		o_  = difference.internalPoint() + difference.position();
		oA_ = difference.domainA().internalPoint() + difference.positionA();
		oB_ = difference.domainB().internalPoint() + difference.positionB();
	}
	virtual void OnPortalFound(const MinkowskiDifference2::SupportPoint& v1Ex, const MinkowskiDifference2::SupportPoint& v2Ex)
	{
		drawSupportLine(v1Ex, v2Ex);
	}
	virtual void OnPortalRefined(const MinkowskiDifference2::SupportPoint& v1Ex, const MinkowskiDifference2::SupportPoint& v2Ex)
	{
		drawSupportLine(v1Ex, v2Ex);
	}
private:
	void drawSupportLine(const MinkowskiDifference2::SupportPoint& v1Ex, const MinkowskiDifference2::SupportPoint& v2Ex)
	{
		::glColor3f(0, 1, 1);
		drawSupportLine(o_, v1Ex.point(), v2Ex.point());
		::glColor3f(1, 0, 1);
		drawSupportLine(oA_, v1Ex.pointOn1(), v2Ex.pointOn1());
		::glColor3f(1, 1, 0);
		drawSupportLine(oB_, v1Ex.pointOn2(), v2Ex.pointOn2());
	}
	void drawSupportLine(const Vector2& o, const Vector2& v1, const Vector2& v2)
	{
		::glBegin(GL_LINE_LOOP);
		::glVertex2d(o.x(), o.y());
		::glVertex2d(v1.x(), v1.y());
		::glVertex2d(v2.x(), v2.y());
		::glEnd();
	}

private:
	Vector2 o_;
	Vector2 oA_;
	Vector2 oB_;
};

static PortalDrawerGL sPortalDrawer;

XenoCollide2::XenoCollide2() : difference_()
{
}

void XenoCollide2::setDomain(const IConvexDomain2& domainA, const Vector2& positionA,
                             const IConvexDomain2& domainB, const Vector2& positionB)
{
	difference_.reset(domainA, positionA, domainB, positionB);
}

bool XenoCollide2::doIntersect(Scalar boundaryTolerance)
{
	MPRKernel2 mpr(difference_, boundaryTolerance);
	mpr.setObserver(sPortalDrawer);
	mpr.run(MPRKernel2::DETECT_BIT);
	return (mpr.doesMeet(MPRKernel2::HIT_BIT));
}

bool XenoCollide2::findContactPoint(Scalar boundaryTolerance,
									Vector2& contactPointA,
									Vector2& contactPointB,
									Vector2& normalA,
									Scalar&  penetrationDepth)
{
	MPRKernel2 mpr(difference_, boundaryTolerance);
	mpr.setObserver(sPortalDrawer);
	mpr.run(MPRKernel2::REACH_BOUNDARY_BIT);
	Vector2 contactPoint;
	mpr.getContactPoint(contactPoint, contactPointA, contactPointB, normalA);
	penetrationDepth = contactPoint.length();
	return (mpr.doesMeet(MPRKernel2::HIT_BIT));
}

bool XenoCollide2::computePenetration(Scalar boundaryTolerance,
									  const Vector2& axis,
									  Vector2& contactPointA,
									  Vector2& contactPointB,
									  Vector2& normalA,
									  Scalar& penetrationDepth)
{
	if (!axis.isZero())
	{
		Vector2 v0 = -axis;
		MPRKernel2 mpr(difference_, boundaryTolerance);
		mpr.setObserver(sPortalDrawer);
		mpr.run(v0, MPRKernel2::REACH_BOUNDARY_BIT);
		Vector2 contactPoint;
		mpr.getContactPoint(contactPoint, contactPointA, contactPointB, normalA);
		penetrationDepth = dot(contactPoint, axis);
		return true;
	}
	else
	{
		assert(!"precondition: axis must not be zero vector");
		return false;
	}
}
